#include "MultiRobotPRM.h"
#include <boost/foreach.hpp>
#include "Conversion.h"

//#define PRINT_STATUS

MultiRobotPRM::MultiRobotPRM(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles, vector<Conf> &start_confs, vector<Conf> &target_confs, Room &room)
    : m_robot_polygons(robot_polygons), m_robot_num(robot_num), m_obstacles(obstacles), m_start_confs(start_confs), m_target_confs(target_confs), m_room(room),
    m_mr_collision_detector(new MultiRobotCollisionDetector(robot_polygons, robot_num, obstacles)), m_mr_sampler(robot_polygons, robot_num, room, *m_mr_collision_detector),
    m_graph(0, true), m_kd_tree(), m_sould_clear(true)
{
    //insert first the start and target points
    m_vertices.push_back(m_start_confs);
    m_vertices.push_back(m_target_confs);
}

MultiRobotPRM::MultiRobotPRM(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles, vector<Conf> &start_confs, vector<Conf> &target_confs, Room &room, MultiRobotCollisionDetector *mr_collision_detector)
    : m_robot_polygons(robot_polygons), m_robot_num(robot_num), m_obstacles(obstacles), m_start_confs(start_confs), m_target_confs(target_confs), m_room(room),
    m_mr_collision_detector(mr_collision_detector), m_mr_sampler(robot_polygons, robot_num, room, *m_mr_collision_detector),
    m_graph(0, true), m_kd_tree(), m_sould_clear(false)
{
    //insert first the start and target points
    m_vertices.push_back(m_start_confs);
    m_vertices.push_back(m_target_confs);
}

MultiRobotPRM::~MultiRobotPRM()
{
    if (m_sould_clear)
    {
        delete m_mr_collision_detector;
    }
}

void MultiRobotPRM::update_path(vector<vector<Conf>> &path)
{
    list<Point_d> found_path;
    m_graph.find_path(m_expandMap[m_start_confs], m_expandMap[m_target_confs], found_path);

    list<Point_d>::const_iterator iter;
    //for each point-index add the point to the generated_path
    for(iter = found_path.begin(); iter != found_path.end(); ++iter)
    {
        path.insert(path.end(), m_map[*iter]);
    }
}

//	This operation is supposed to generate the roadmap (sample configurations
//	and connect them.
// number_vertices: number of sampled vertices
// k_nearest:maximal degree of vertex in roadmap
void MultiRobotPRM::generate_roadmap(int number_vertices, int	k_nearest, double eps)
{
    //generate number_vertices samples
#ifdef PRINT_STATUS
    std::cout << "  0%";
#endif
    int index = 0;
    for (index = 0; index < number_vertices; ++index)
    {
        m_vertices.push_back(m_mr_sampler.generate_sample());
#ifdef PRINT_STATUS
        if (index % 5 == 0)
        {
            int per = (int)(((1.0 * index) / number_vertices) * 100);
            if (per == 100)
            {
                per = 99;
            }
            if (per < 10)
            {
                std::cout << "\x08\x08" << per << "%";
            }
            else
            {
                std::cout << "\x08\x08\x08" << per << "%";
            }
        }
#endif
    }
    
    //Add them to the graph and insert to the kd-tree
    BOOST_FOREACH(vector<Conf>& points, m_vertices)
    {
        Point_d point_d = Conversion::to_Point_d(points);

        m_expandMap[points] = point_d;
        m_map[point_d] = points;

        m_kd_tree.insert(point_d);
        m_graph.add_vertex(m_expandMap[points]);
    }

#ifdef PRINT_STATUS
    std::cout << "\x08\x08\x08" << "100%  0%";
#endif
    
    index = 0;
    // go over all the points
    BOOST_FOREACH(vector<Conf>& point, m_vertices){
        Point_d& p = m_expandMap[point];
        vector<Point_d> nearest_neighbors;
        // find the k- nearest neighbors
        m_kd_tree.k_nearest_neighbors(p, k_nearest, std::back_inserter(nearest_neighbors));
        BOOST_FOREACH(Point_d& q, nearest_neighbors){
            // if there isn't an edge between them (or the connected components) and there is a valid-path (by the local planner) between them
            if (!m_graph.is_in_same_cc(p, q) && m_mr_collision_detector->local_planner(m_map[p], m_map[q], eps))
            {
                // add an edge to the graph
                m_graph.add_edge(p, q);
            }
        }
#ifdef PRINT_STATUS
        if (index % 5 == 0)
        {
            int per = (int)(((1.0 * index) / number_vertices) * 100);
            if (per == 100)
            {
                per = 99;
            }
            if (per < 10)
            {
                std::cout << "\x08\x08" << per << "%";
            }
            else
            {
                std::cout << "\x08\x08\x08" << per << "%";
            }
        }
        ++index;
#endif
    }
#ifdef PRINT_STATUS
    std::cout << "\x08\x08\x08\x08\x08\x08\x08\x08\x08";
#endif
}

vector<Conf> MultiRobotPRM::generate_valid_sample()
{
    return m_mr_sampler.generate_sample();
}
